//Function libraries for the Skytank

static const int INSTR_LEN = 4;
static const int PARAM_LEN = 4;

struct Command
{
  char instruction[INSTR_LEN + 1];   //4 characters, 1 null terminator
  char parameter[INSTR_LEN + 1];     //4 characters, 1 null terminator
};

void play_sound(int pin, unsigned int freq, int dur)
{
  tone(pin, freq);
  delay(dur);
  noTone(pin); 
}

void init_play(int pin)
{
  //This is the initial sound played
  for(int i = 10; i < 750; i = i+15)
  {  
    int ran = i + random(-30, 10);
    if(ran < 0) ran = 0;
    play_sound(pin, ran, 50);
  } 
  noTone(pin);
}

boolean listen_serial(Command& cmd)
{
  int buf_vals = Serial.available();
  //Read the buffer for 1 instruction 
  if(buf_vals >= INSTR_LEN + PARAM_LEN) 
  {
   //Command found: Read command and parameters
   for(int i = 0; i < INSTR_LEN; i++) {cmd.instruction[i] = Serial.read();} 
   for(int i = 0; i < PARAM_LEN; i++) {cmd.parameter[i] = Serial.read();}
   return true;
  }
  return false;
}

int parse_instruction(char* instruction)
{
  int instr_num = -1;
  if(strcmp(instruction, "INIT\0") == 0)      instr_num = 0;  //SkytanX handshake sequence
  else if(strcmp(instruction, "LEDC\0") == 0) instr_num = 1;  //LED Control scheme
  else if(strcmp(instruction, "MOVE\0") == 0) instr_num = 2;  //Presents a move command
  else if(strcmp(instruction, "HALT\0") == 0) instr_num = 3;  //Immediately stops the car
  else if(strcmp(instruction, "DBUG\0") == 0) instr_num = 4;  //Prints a set of outputs for Debugging mode. Not enabled for normal use.
  else if(strcmp(instruction, "TEST\0") == 0) instr_num = 5;  //Test instruction, suitable for debugging specific functions
  else                                        instr_num = -1; //Failed to get the right code.
  return instr_num;
}

//-----------------------------------------------
//MOTOR CONTROLS
//-----------------------------------------------
void moveMotors(int LSpeed, int RSpeed)
{
 if(debug) 
  {
    Serial.print("LSpeed: "); Serial.print(LSpeed);
    Serial.print(" RSpeed: "); Serial.println(RSpeed); 
  }
 //Left motor control
 if(LSpeed > 0 && (!FL_Stop || !FR_Stop)) {analogWrite(LMotorForw, LSpeed); analogWrite(LMotorBack, 0);}
 else if (!BL_Stop || !BR_Stop)           {analogWrite(LMotorForw, 0); analogWrite(LMotorBack, LSpeed);}
 //Right motor  control
 if(RSpeed > 0 && (!FL_Stop || !FR_Stop)) {analogWrite(RMotorForw, RSpeed); analogWrite(RMotorBack, 0);}
 else if (!BL_Stop || !BR_Stop)           {analogWrite(RMotorForw, 0); analogWrite(RMotorBack, RSpeed);}
 //delay(250);
}

boolean detect_cliff()
{
  //Detect for cliff detection
  //Here we must keep track of time + the cliff detection units.
  //if(debug) Serial.println("Running cliff detect algorithm");
  IR_FL = analogRead(1); //Front Left
  IR_FR = analogRead(0); //Front Right
  IR_BL = analogRead(2); //Back Left
  IR_BR = analogRead(3); //Back Right
  //Detect whether the cliff exists
  if(IR_FL_Base - IR_FL > 100) FL_Stop = true; else FL_Stop = false;
  if(IR_FR_Base - IR_FR > 100) FR_Stop = true; else FR_Stop = false;
  if(IR_BL_Base - IR_BL > 100) BL_Stop = true; else BL_Stop = false;
  if(IR_BR_Base - IR_BR > 100) BR_Stop = true; else BR_Stop = false;
  
  if(FL_Stop || FR_Stop || BL_Stop || BR_Stop) 
  {
    moveMotors(0, 0); //Halt the car if cliff hits
    if(debug) Serial.println("MOVE FAILURE");
    //Serial.println("FAILMOVE"); //Simple failure code
    Serial.print("FMOV"); Serial.print(int(FL_Stop)); Serial.print(int(FR_Stop)); Serial.print(int(BL_Stop)); Serial.print(int(BR_Stop));
    return 1;
  }
  return 0;
}

void print_debug()
{
  Serial.println("Base IR Sensors Readings");
  Serial.print("FL: ");  Serial.print(IR_FL_Base);
  Serial.print(" FR: "); Serial.print(IR_FR_Base);  
  Serial.print(" BL: "); Serial.print(IR_BL_Base);
  Serial.print(" BR: "); Serial.println(IR_BR_Base);    

  Serial.println("Current IR Sensors Readings");
  Serial.print("FL: ");  Serial.print(IR_FL);
  Serial.print(" FR: "); Serial.print(IR_FR);  
  Serial.print(" BL: "); Serial.print(IR_BL);
  Serial.print(" BR: "); Serial.println(IR_BR);    
  
  Serial.println("Cliff Detect Flags");
  Serial.print("FL: ");  Serial.print((int)FL_Stop);
  Serial.print(" FR: "); Serial.print((int)FR_Stop);  
  Serial.print(" BL: "); Serial.print((int)BL_Stop);
  Serial.print(" BR: "); Serial.println((int)BR_Stop);    
  
  delay(5000);
}

void run_test()
{
  if(debug) Serial.println("Running Test Code.");
  analogWrite(RMotorForw, 255); analogWrite(RMotorBack, 0);
  analogWrite(LMotorForw, 0); analogWrite(LMotorBack, 255);
}

//ADD ENUMS LATER - LOVE, MICHAEL
void update(Command& cmd)
{
  int instr = parse_instruction(cmd.instruction);
  switch(instr)
  {
    case -1: //Failed to find the right code
     Serial.println("FAILCODE");
     break;
    case 0: //Handshake message
     Serial.println("ARDY0000");
     if(debug) Serial.println("Arduino Ready for commands.");
     init_play(4); //Startup Sound!
     ardu_ready = true;
     break;
    case 1: //LED control message
     digitalWrite(13, HIGH);
     break;
    case 2: //Motor control message
     { //Needed this brace to allow for initialization
       //For now, let's have just 0 - 9 denote speed. 0 - 3 for back, 4 for stop, 5 - 9 for forward
       if(debug) Serial.println("Motor command received");
       int left = (int)((cmd.parameter[1] - 52)*50);    //2nd byte
       int right = (int)((cmd.parameter[2] - 52)*50);   //3rd byte
       int delaytime = (int)((cmd.parameter[3] - 48));  //4th byte     
       //moveMotors(left, right); //Start moving the motors
       if(delaytime == 0) {delaytime = -1;} //Set it in infinite mode
       int start_time = 0; //Keep track of time if we need to.
       if(delaytime != -1) //Detect for times up
       {
         if(debug) Serial.println("Timer Started for motor control.");
         start_time = millis(); 
         int total_delay = delaytime*1000;
         moveMotors(left, right);
         while(millis() - start_time < total_delay && !detect_cliff()) 
         {
           if(debug) Serial.println(millis() - start_time);
           delay(20);
         }
         moveMotors(0, 0); //Stop the motors afterwards.
       }
       else {moveMotors(left, right);}
       if(debug)
       {
          Serial.print("Parameters: "); Serial.print(cmd.parameter[1]); Serial.print(cmd.parameter[2]); Serial.println(cmd.parameter[3]);
       }
     break;
     }
    case 3:  //HALT: Stops the robot entirely
     moveMotors(0,0);
     break;
    case 4:  //DEBUG: Do a printout of our variables
     print_debug();
     break;
    case 5:  //Space for testing command and code
     run_test();
     break;
  }
}

void execute()
{
  detect_cliff(); //We need to detect if there are any cliffs and set various flags
}
